# -*- coding:utf-8 -*- 
import cv2
import imutils
import time
import numpy as np
import V_UCom as com

# Send_data
Uart_buf = [0x55,0xAA,0x00,0x00,0x00,0x00,0x00,0x00,
            0x00,0x00,0x00,0xAA]

# set com
com.init(mode=2)
# setup BlobDetector parameter
params = cv2.SimpleBlobDetector_Params()

# set min and max
params.minThreshold = 20
params.maxThreshold = 200

# Filter
params.filterByColor = True
params.blobColor = 0

ver = (cv2.__version__).split('.')
if int(ver[0]) < 3:
    dete = cv2.SimpleBlobDetector(params)
else:
    dete = cv2.SimpleBlobDetector_create(params)

cap = cv2.VideoCapture(0)
cap.set(3, 320)
cap.set(4, 240)
print("start reading video")
time.sleep(2.0)
print("start working")

flymode = 0x10

T = time.time()

while(True):
    ret, frame = cap.read()
    frame = imutils.resize(frame, width=160)
    keypoints = dete.detect(frame)
    if (keypoints):
        for i in range(0, len(keypoints)):
            x = keypoints[i].pt[0]
            y = keypoints[i].pt[1]
            px = int(x)
            py = int(y)
    else:
        px = 80
        py = 60
    if (time.time() - T) > 10:
        flymode = 0x11
    Uart_buf = [0x55, 0xaa, flymode, px>>8, px & 0x00ff, py>>8, py & 0x00ff, 0x00, 0x00, 0x00, 0x00, 0xaa]
    Uart_bufb = bytearray(Uart_buf)
    frame_show = cv2.drawKeypoints(frame.copy(), keypoints, np.array([]), (255, 255, 255), cv2.DRAW_MATCHES_FLAGS_NOT_DRAW_SINGLE_POINTS)
    cv2.imshow("frame", frame_show)
    com.send(Uart_bufb)
    print(px, py, Uart_buf[2])
    # print(Uart_buf)
    cv2.waitKey(1)
cv2.destroyAllWindows()
